#!/usr/bin/env python3
import rospy
from drone_pkg.msg import QRCode
import cv2
import numpy as np
from pyzbar.pyzbar import decode
import os
import time




def Laser_Init():
        os.system("gpio mode 10 out")
        os.system("gpio mode 13 out")
        os.system("gpio write 10 1")
        os.system("gpio write 13 1")

def Laser_Control(laser_id,state):
    if laser_id == 1 and state == 0:
        os.system("gpio write 10 1")
    if laser_id == 1 and state == 1:
        os.system("gpio write 10 0")
    if laser_id == 0 and state == 0:
        os.system("gpio write 13 1")
    if laser_id == 0 and state == 1:
        os.system("gpio write 13 0")

def Laser_ON(laser_id, t):
     Laser_Control(laser_id, 1)
     time.sleep(t * 0.1)
     Laser_Control(laser_id, 0)

def shutdown():
     Laser_Control(1, 0)
     Laser_Control(0, 0)
     os.system("gpio write 19 1")
     cv2.destroyAllWindows()  

def Read_Decode_Pic(name, image, CamID):
    global pub
    max_area = 0
    max_code = None
    for code in decode(image):
        data = code.data.decode('utf-8')
        pts_poly = np.array(code.polygon, np.int32)  
        cv2.polylines(image, [pts_poly], True, (0, 255, 0), 5)  
        pts_rect = code.rect
        
        # 计算二维码的面积
        area = pts_rect[2] * pts_rect[3]
        
        # 找到面积最大的二维码
        if area > max_area:
            max_area = area
            max_code = {
                'data': data,
                'pts_rect': pts_rect,
                'cam_id': CamID
            }

    if max_code:
        # 获取最大的二维码的相关信息
        data = max_code['data']
        pts_rect = max_code['pts_rect']
        CamID = max_code['cam_id']
        rospy.loginfo("%d, %d", int(CamID), int(data))
        # 标记最大二维码
        cv2.putText(image, data, (pts_rect[0], pts_rect[1]), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
        center_x = pts_rect[0] + pts_rect[2] // 2
        center_y = pts_rect[1] + pts_rect[3] // 2
    #     在二维码中心画一个红点
        cv2.circle(image, (center_x, center_y), 5, (0, 0, 255), -1)  # 红色圆点

        text = f'Center: ({center_x}, {center_y}), Width: {pts_rect[2]}, Height: {pts_rect[3]}'
        cv2.putText(image, text, (center_x + 10, center_y - 10), 
                    cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)

        
        # 发送最大二维码的信息
        msg = QRCode()
        msg.Data = str(data)
        msg.CamID = CamID
        msg.x = pts_rect[0]
        msg.y = pts_rect[1]
        msg.Width = pts_rect[2]
        msg.Height = pts_rect[3]
        pub.publish(msg)
    
    cv2.imshow(name, image)   
# def Read_Decode_Pic(name, image, CamID):
#     global pub
#     max_area = 0
#     max_code = None
#     for code in decode(image):
#         data = code.data.decode('utf-8')
#         pts_poly = np.array(code.polygon, np.int32)  
#         cv2.polylines(image, [pts_poly], True, (0, 255, 0), 5)  
#         pts_rect = code.rect
#         center_x = pts_rect[0] + pts_rect[2] // 2
#         center_y = pts_rect[1] + pts_rect[3] // 2
#         cv2.circle(image, (center_x, center_y), 5, (0, 0, 255), -1)  # 红色圆点

#         # 在二维码旁边写下中心坐标
#         cv2.putText(image, f'Center: ({center_x}, {center_y})', (center_x + 10, center_y - 10), 
#                     cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)

        
#         # 计算二维码的面积
#     #    area = pts_rect[2] * pts_rect[3]
        
#     #     # 找到面积最大的二维码
#     #     if area > max_area:
#     #         max_area = area
#     #         max_code = {
#     #             'data': data,
#     #             'pts_rect': pts_rect,
#     #             'cam_id': CamID
#     #         }

#     # if max_code:
#     #     # 获取最大的二维码的相关信息
#     #     data = max_code['data']
#     #     pts_rect = max_code['pts_rect']
#     #     CamID = max_code['cam_id']
        
#     #     # 计算二维码中心坐标
#     #     center_x = pts_rect[0] + pts_rect[2] // 2
#     #     center_y = pts_rect[1] + pts_rect[3] // 2

#     #     rospy.loginfo("%d, %d", int(CamID), int(data))
        
#     #     # 标记最大二维码
#     #     cv2.putText(image, data, (pts_rect[0], pts_rect[1]), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
        
#     #     # 在二维码中心画一个红点
#     #     cv2.circle(image, (center_x, center_y), 5, (0, 0, 255), -1)  # 红色圆点

#     #     # 在二维码旁边写下中心坐标
#     #     cv2.putText(image, f'Center: ({center_x}, {center_y})', (center_x + 10, center_y - 10), 
#     #                 cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)

#     #     # 发送最大二维码的信息
#     #     msg = QRCode()
#     #     msg.Data = str(data)
#     #     msg.CamID = CamID
#     #     msg.x = pts_rect[0]
#     #     msg.y = pts_rect[1]
#     #     msg.Width = pts_rect[2]
#     #     msg.Height = pts_rect[3]
#     #     pub.publish(msg)
    
#     cv2.imshow(name, image)



if __name__ == '__main__':
    rospy.init_node("QRCode_detect", anonymous=True)
    rospy.on_shutdown(shutdown)
    os.system("gpio mode 19 out")
    os.system("gpio write 19 1")
    pub = rospy.Publisher("/QRCode_detect", QRCode, queue_size=10)
    rate = rospy.Rate(100)
    # cap0 = cv2.VideoCapture('/dev/came0')
    cap1 = cv2.VideoCapture('/dev/came1')
    # ret, frame0 = cap0.read()
    
    count = 0
    flag = 0
    while not rospy.is_shutdown():
        ret, frame1 = cap1.read()
        count += 1
        try:   
            Read_Decode_Pic('1', frame1, 1) 
            if (count >= 10):
                count = 0
                flag = 1-flag
                if (flag):
                    os.system("gpio write 19 0")
                else:
                    os.system("gpio write 19 1")

        except Exception as e:
            print(f"Error: {e}")

        cv2.waitKey(1)
        rate.sleep()

    cv2.destroyAllWindows()       
    
